/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package framework;


import commands.CommandBase;
import commands.DriveWithJoystick;
import commands.LeftBrake;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.PIDCommand;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotTemplate extends IterativeRobot {
    
    
    

    Command driveRobot = new DriveWithJoystick();
    PIDCommand left_ebrake = new LeftBrake(HW.Kp,HW.Ki,HW.Kd);
  
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        // instantiate the command used for the autonomous period
       
       
        
        // Initialize all subsystems
        CommandBase.init();
        
         NetworkTable.initialize();
        SmartDashboard.putData(left_ebrake);
        updateDashboard();
        
        
        

    }
    
    public void disabledContinuous(){
    
        updateDashboard();
        Timer.delay(0.39218757);
        
    }
    
    public void disabledInit(){
        CommandBase.drivetrain.getLeftEncoder().reset();
        
    }
    
    public void disabledPeriodic(){
        
    }

    public void autonomousInit() {
        //System.out.println("SPI OUTPUT: ");
        left_ebrake.start();
        // schedule the autonomous command (example)

    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        updateDashboard();
        Scheduler.getInstance().run();
        
    }

    public void teleopInit() {
		// This makes sure that the autonomous stops running when
		// teleop starts running. If you want the autonomous to 
		// continue until interrupted by another command, remove
		// this line or comment it out.
                left_ebrake.cancel();
                driveRobot.start();
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
    //    System.out.println("Infrared Sensor Value: " + CommandBase.collector.getIRSensor().getValue());
        updateDashboard();
        Scheduler.getInstance().run();
    }
    
    public void updateDashboard(){
        
        SmartDashboard.putInt("Infrared Sensor Value: ", CommandBase.collector.getIRSensor().getValue());
        
        SmartDashboard.putDouble("Robot Speed(MPH)",Math.abs(CommandBase.drivetrain.getLeftEncoder().getRate()*0.0568181818));
        SmartDashboard.putDouble("Robot Speed(M/S)",Math.abs(CommandBase.drivetrain.getLeftEncoder().getRate()*0.0254));
        SmartDashboard.putDouble("Distance Traveled: ",CommandBase.drivetrain.getLeftEncoder().getDistance());
        
        for(int i =1; i <= CommandBase.drivetrain.getJagArr().length;i++)
        {
            try{
            CANJaguar jag = CommandBase.drivetrain.getJaguar(i);
            SmartDashboard.putString("Jaguar " + i, jag.getDescription());
            SmartDashboard.putDouble("Temp",jag.getTemperature());
            SmartDashboard.putDouble("Bus Voltage",jag.getBusVoltage());
            SmartDashboard.putDouble("Output Current",jag.getOutputCurrent());
            SmartDashboard.putDouble("Output Voltage",jag.getOutputVoltage());
            SmartDashboard.putDouble("Raw Speed",jag.getX());
            }catch(Exception e){System.out.println(e.getMessage());}
        }
    }
}
